﻿/** 本程序暂时如何旋转欧拉角 */

#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkMatrix3x3.h>
#include <vtkMath.h>
#include <QString>

#include <Eigen/Core>
#include <Eigen/Dense>

/** 获取欧拉角的transfrom */
vtkSmartPointer<vtkTransform> getEulerTransformZYX(double angleX, double angleY, double angleZ)
{
    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
    transform->PreMultiply();  // M = RotZ * RotY * RotX

    transform->RotateZ(angleZ);
    transform->RotateY(angleY);
    transform->RotateX(angleX);

    return transform;
}

/** 欧拉角反转回去，首先绕反转后的X轴转w，然后依次v，最后u */
vtkSmartPointer<vtkTransform> getUvwTransform(double u, double v, double w)
{
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
    transform->PostMultiply();
    transform->RotateZ(u);

    double normalY[3] = {0, 1, 0};
    double axisY[3];
    transform->TransformNormal(normalY, axisY);
    transform->RotateWXYZ(v, axisY);

    double normalX[3] = {1, 0, 0};
    double axisX[3];
    transform->TransformNormal(normalX, axisX);
    transform->RotateWXYZ(w, axisX);

    return transform;
}

bool isRotationMatrix(vtkMatrix3x3 *m)
{
    vtkSmartPointer<vtkMatrix3x3> mT = vtkSmartPointer<vtkMatrix3x3>::New();
    vtkSmartPointer<vtkMatrix3x3> mIdentity = vtkSmartPointer<vtkMatrix3x3>::New();
    vtkMatrix3x3::Transpose(m, mT);
    vtkMatrix3x3::Multiply3x3(m, mT, mIdentity);

    return mIdentity->IsIdentity();
}

void getEulerAngle(vtkMatrix4x4 *m, double *eulerAngle)
{
    /**
     u, v, w 绕 z, y, x
    [
        cv * cu     sw*sv*cu - cw*su   cw*sv*cu - sw*su
        cv * su     sw*sv*su + cw*cu   cw*sv*su - sw*cu
        -sv         sw * cv            cw * cv
    ]
     */
    double u = 0, v= 0, w=0;

    // r11*r11 + r21*r21 不能为0
    double sy = sqrt(m->GetElement(0,0) * m->GetElement(0,0) +  m->GetElement(1,0) * m->GetElement(1,0) );

    bool singular = sy < 1e-6;

    if (singular) {
        // 当cv = 0, sv = 1/-1时候
        w = atan2(-m->GetElement(1,2), m->GetElement(1,1));
        v = atan2(-m->GetElement(2,0), sy);
        u = 0;
    } else {
        w = atan2(m->GetElement(2, 1), m->GetElement(2,2));  // 当 y 旋转角度不为90的时候
        v = atan2(-m->GetElement(2,0), sy);
        u = atan2(m->GetElement(1,0), m->GetElement(0,0));
    }

    eulerAngle[2] = vtkMath::DegreesFromRadians(u) ;
    eulerAngle[1] = vtkMath::DegreesFromRadians(v);
    eulerAngle[0] = vtkMath::DegreesFromRadians(w);
}

/// Eigen库验证
static void eigenAngleAxis(double angleX, double angleY, double angleZ)
{
    cout << "demoAngleAxis:" << endl;
    // yaw-z pitch-y roll-x,并转成弧度
    float yaw = angleZ * M_PI / 180;
    float pitch = angleY * M_PI / 180;
    float roll = angleX * M_PI / 180;
    Eigen::Matrix3f m;
    m = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
    cout << m << endl;
    cout << "is unitary:" << m.isUnitary() << endl;
    Eigen::AngleAxisf aa;
    aa.fromRotationMatrix(m);
    cout << aa.toRotationMatrix() << endl;

    // to euler angle Z-Y-X
    Eigen::Vector3f eulerAngle = m.eulerAngles(2, 1, 0);
    cout << "eulerAngle 顺序为 ZYX: " << eulerAngle.transpose() << endl;
}

int main(int, char *[])
{
    double x = 60.0, y = 45.0, z = 36.0;

    std::cout << "先用Eigen库验证:" << endl;
    eigenAngleAxis(x, y, z);

    std::cout << "计算生成通过旋转脚生成的矩阵:" << endl;

    vtkSmartPointer<vtkTransform> transformZYX = getEulerTransformZYX(x, y, z);
    transformZYX->GetMatrix()->Print(std::cout);


    vtkSmartPointer<vtkMatrix4x4> m =
            vtkSmartPointer<vtkMatrix4x4>::New();
    m->DeepCopy(transformZYX->GetMatrix());
    //std::cout << "Euler ZYX matrix:" << *m << endl;


    // 获取欧拉角
    std::cout << "获取欧拉角:" << std::endl;
    double eulerAng[3];
    getEulerAngle(m, eulerAng);
    std::cout<< QString::asprintf("uvw: %lf, %lf, %lf", eulerAng[0], eulerAng[1], eulerAng[2]).toLocal8Bit().data() << endl;

    //vtkSmartPointer<vtkTransform> transformUvw = getUvwTransform(u, v, w);
    //vtkMatrix4x4 *mUvw = transformUvw->GetMatrix();

    // std::cout << "Uvw matrix:" << *mUvw << endl;

    return EXIT_SUCCESS;
}
